% 清空工作区
%clear; clc; close all;

% 定义四旋翼无人机参数
mass_nominal = 0.65; % 质量 (kg)
mass_range = [0.6 0.7];
g = 9.81; % 重力加速度 (m/s^2)
Ixx_nominal = 7.5E-3; Iyy_nominal = 7.5E-3; Izz_nominal = 1.3E-2; % 惯性矩 (kg·m^2)
Ixx_range = [7e-3 8e-3];
Iyy_range = [7e-3 8e-3];
Izz_range = [1.2e-2 1.4e-2];

% 定义状态和输入维度
nx = 12; % 状态维度 [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
nu = 4; % 输入维度 [F, tau_phi, tau_theta, tau_psi]
ny = 4; % 输出维度 [x, y, z, psi]


% 定义NMPC控制器
Ts = 0.1; % 采样时间
nlobj = nlmpc(nx, ny, nu); % 创建NMPC对象
nlobj.Ts = Ts; % 采样时间
nlobj.PredictionHorizon = 10; % 预测步长
nlobj.ControlHorizon = 2; % 控制步长

% 定义状态方程
nlobj.Model.StateFcn = @(x, u) quadrotorStateFcn(x, u, mass_nominal, Ixx_nominal, Iyy_nominal, Izz_nominal, g);

% 定义输出方程 (假设输出为状态的前6个变量: [x, y, z, vx, vy, vz])
nlobj.Model.OutputFcn = @(x, u) [x(1); x(2); x(3); x(9)];

% 定义约束
nlobj.Weights.OutputVariables = [1 1 1 1]; % 输出权重
nlobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1]; % 输入权重
nlobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1]; % 输入变化率权重

% 定义输入约束
nlobj.ManipulatedVariables(1).Min = 0; % 最小推力
nlobj.ManipulatedVariables(1).Max = 20; % 最大推力
nlobj.ManipulatedVariables(2).Min = -5; % 最小滚转力矩
nlobj.ManipulatedVariables(2).Max = 5; % 最大滚转力矩
nlobj.ManipulatedVariables(3).Min = -5; % 最小俯仰力矩
nlobj.ManipulatedVariables(3).Max = 5; % 最大俯仰力矩
nlobj.ManipulatedVariables(4).Min = -5; % 最小偏航力矩
nlobj.ManipulatedVariables(4).Max = 5; % 最大偏航力矩

% 验证NMPC控制器
validateFcns(nlobj, rand(nx, 1), rand(nu, 1));

% 定义参考轨迹
T = 250; % 仿真时间
t = 0:Ts:T; % 时间向量
ref_traj = [sin(0.1*t); cos(0.1*t); 0.5*t; 45*3.14/180*ones(size(t))]; % 参考轨迹 [x; y; z; psi]

% 初始化状态和控制输入
x0 = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % 初始状态
u0 = [mass_nominal * g; 0; 0; 0]; % 初始输入 (悬停状态)

% 初始化历史记录
X = x0'; % 状态历史
U = u0'; % 输入历史
Y = [x0(1) x0(2) x0(3) x0(9)]; % 输出历史
mass_history = [];

% 仿真循环
for k = 1:length(t)
    % 获取当前参考轨迹
    ref = ref_traj(:, k)'; % 当前参考输出
    
    % 计算控制输入
    [u, info] = nlmpcmove(nlobj, x0, u0, ref);
    
    % 更新状态 (使用四阶Runge-Kutta方法)
    % 随机生成参数扰动
    ratio = rand;
    mass = mass_range(1)*ratio + mass_range(2)*(1-ratio);
    Ixx = Ixx_range(1)*ratio + Ixx_range(2)*(1-ratio);
    Iyy = Iyy_range(1)*ratio + Iyy_range(2)*(1-ratio);
    Izz = Izz_range(1)*ratio + Izz_range(2)*(1-ratio);
    x0 = rk4(@(x, u) quadrotorStateFcn(x, u, mass, Ixx, Iyy, Izz, g), x0, u, Ts);
    
    % 保存结果
    X = [X; x0'];
    U = [U; u'];
    Y = [Y; [x0(1); x0(2); x0(3); x0(9)]'];
    mass_history = [mass_history; mass];
    
    % 更新控制输入
    u0 = u;
end
%% 绘制结果
close all;
figure;
subplot(4, 1, 1);
plot(t, Y(1:size(t,2), 1), 'LineWidth', 2); hold on;
plot(t, ref_traj(1, :), 'r--'); 
xlabel('Time (s)');
ylabel('Position (m)');
legend('仿真轨迹', '期望轨迹');
title('X Position Tracking');
grid on;

subplot(4, 1, 2);
plot(t, Y(1:size(t,2), 2), 'LineWidth', 2); hold on;
plot(t, ref_traj(2, :), 'r--'); 
xlabel('Time (s)');
ylabel('Position (m)');
legend('仿真轨迹', '期望轨迹');
title('Y Position Tracking');
grid on;

subplot(4, 1, 3);
plot(t, Y(1:size(t,2), 3), 'LineWidth', 2); hold on;
plot(t, ref_traj(3, :), 'r--'); 
xlabel('Time (s)');
ylabel('Position (m)');
legend('仿真轨迹', '期望轨迹');
title('Z Position Tracking');
grid on;

subplot(4, 1, 4);
plot(t, Y(1:size(t,2), 4), 'LineWidth', 2); hold on;
plot(t, ref_traj(4, :), 'r--'); 
xlabel('Time (s)');
ylabel('Position (m)');
legend('仿真轨迹', '期望轨迹');
title('Yaw Position Tracking');
grid on;

% figure;
% plot3(Y(:, 1), Y(:, 2), Y(:, 3), 'b-', 'LineWidth', 2); hold on;
% plot3(ref_traj(1, :), ref_traj(2, :), ref_traj(3, :), 'r--', 'LineWidth', 2);
% xlabel('X Position (m)');
% ylabel('Y Position (m)');
% zlabel('Z Position (m)');
% legend('仿真轨迹', '期望轨迹');
% title('3D Trajectory Tracking');
% grid on;

figure;
subplot(4, 1, 1);
plot(t, 0.1*Y(1:size(t,2), 1)-0.1*ref_traj(1, :)', 'LineWidth', 2); hold on;
xlabel('Error in x (m)');
grid on;
% ylabel('Position (m)');
% title('X Position Tracking');

subplot(4, 1, 2);
plot(t,0.1* Y(1:size(t,2), 2)-0.1*ref_traj(2, :)', 'LineWidth', 2); hold on;
xlabel('Error in y (m)');
grid on;
% ylabel('Position (m)');
% title('Y Position Tracking');

subplot(4, 1, 3);
plot(t, 0.1*Y(1:size(t,2), 3)-0.1*ref_traj(3, :)', 'LineWidth', 2); hold on;
xlabel('Error in z (m)');
grid on;
% ylabel('Position (m)');
% title('Z Position Tracking');

subplot(4, 1, 4);
plot(t, 0.1*Y(1:size(t,2), 4)-0.1*ref_traj(4, :)', 'LineWidth', 2); hold on;
xlabel('Error in yaw (rad)');
grid on;

figure;
plot(t, mass_history');
xlabel('actual mass (kg)');
grid on;
%% 四旋翼无人机状态方程
function dxdt = quadrotorStateFcn(x, u, mass, Ixx, Iyy, Izz, g)
    % 状态变量
    vx = x(4); vy = x(5); vz = x(6);
    phi = x(7); theta = x(8); psi = x(9);
    p = x(10); q = x(11); r = x(12);
    
    % 输入变量
    F = u(1); tau_phi = u(2); tau_theta = u(3); tau_psi = u(4);
    
    % 动力学方程
    dxdt = zeros(12, 1);
    dxdt(1) = vx; % x 位置
    dxdt(2) = vy; % y 位置
    dxdt(3) = vz; % z 位置
    dxdt(4) = (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) * F / mass; % vx
    dxdt(5) = (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) * F / mass; % vy
    dxdt(6) = (cos(phi)*cos(theta)) * F / mass - g; % vz
    dxdt(7) = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r; % phi
    dxdt(8) = cos(phi)*q - sin(phi)*r; % theta
    dxdt(9) = sin(phi)/cos(theta)*q + cos(phi)/cos(theta)*r; % psi
    dxdt(10) = (Iyy - Izz)/Ixx * q * r + tau_phi/Ixx; % p
    dxdt(11) = (Izz - Ixx)/Iyy * p * r + tau_theta/Iyy; % q
    dxdt(12) = (Ixx - Iyy)/Izz * p * q + tau_psi/Izz; % r

end

%% 四阶Runge-Kutta方法
function x_next = rk4(dynamics, x, u, Ts)
    k1 = dynamics(x, u);
    k2 = dynamics(x + 0.5*Ts*k1, u);
    k3 = dynamics(x + 0.5*Ts*k2, u);
    k4 = dynamics(x + Ts*k3, u);
    x_next = x + (Ts/6) * (k1 + 2*k2 + 2*k3 + k4);
end